use cmsis_rtos2_rs::{osWaitForever, os_delay, static_mem::EventFlags};
use driver_utils::service::simple_step_motor::{Dir, StepMotor, StepMotorDrv};
use periph_utils::{ stm32f1::stm32f103::{interrupt, Interrupt}, GpioConfig, TimPwm};
use periph_utils::stm32f1;
use periph_utils::stm32f1::stm32f103::NVIC;
use utils::MutCell;

use crate::cp_cargo::CargoErr;

use super::sensor;

#[derive(GpioConfig)]
#[gpio(init = "gpio_init",
    io(name = "dir", io = "PC3", mode = "OUT_PP", set, reset),
    io(name = "enable", io = "PC5", mode = "OUT_PP", set, reset),
    io(name = "alm", io = "PC4", mode = "IN_FLOAT", read),
)]
struct Pin;

#[derive(GpioConfig)]
#[gpio(init = "gpio_init", io(name = "puls", io = "PA1", mode = "OUT_PP", set, reset))]
struct PulsPin;

#[derive(TimPwm, GpioConfig)]
#[gpio(init = "gpio_init", io(name = "puls", io = "PA1", mode = "AF_PP"))]
#[pwm(init = "pwm_init", tim = "TIM2", freq = 100, set_freq, enable, disable, irq,
    ch(ch = 2, duty = 50, mode = 1, set_duty)
)]
struct PWM;

struct PickMotorDrv;

static PICK_MOTOR: MutCell<StepMotor<PickMotorDrv>> = MutCell::const_new(StepMotor::const_new(PickMotorDrv));
static EVENT_FLAGS: EventFlags = EventFlags::uninit();
const FLAG_FINISH: u32 = 0x01;

impl StepMotorDrv for PickMotorDrv {
    type Error = CargoErr;
    const TIMEOUT_ERR: Self::Error = CargoErr::PickTimeout;
    const ALM_ERR: Self::Error = CargoErr::PickAlm;
    const ACC: u32 = 4;
    const BASE_FREQ: u32 = 250;
    const VAR_RANGE: u32 = 600;
    const RESET_FREQ: u32 = 1500;
    const RESET_TIMEOUT: u32 = 30 * 1000;
    const SLOW_DELAY_TICK: u32 = 5;
    const SLOW_DELAY: u32 = 200;

    fn delay(tick: u32) {
        os_delay(tick);
    }

    fn disable_pwm() {
        PWM::tim2_pwm_disable_irq();
        PWM::tim2_pwm_disable();
        PulsPin::gpio_init_no_rcc();
    }

    fn enable_pwm(freq: u32) {
        PWM::gpio_init_no_rcc();
        PWM::tim2_pwm_set_freq(freq);
        PWM::tim2_pwm_enable_irq();
        PWM::tim2_pwm_enable();
    }

    fn enable_pwm_not_irq(freq: u32) {
        PWM::gpio_init_no_rcc();
        PWM::tim2_pwm_set_freq(freq);
        PWM::tim2_pwm_disable_irq();
        PWM::tim2_pwm_enable();
    }

    fn is_alm() -> bool {
        !Pin::alm_read()
    }

    fn is_origin() -> bool {
        sensor::is_pick_motor_origin()
    }

    fn reset_puls_pin() {
        PulsPin::puls_reset();
    }

    fn set_dir(dir: Dir) {
        match dir {
            Dir::Enter => Pin::dir_reset(),
            Dir::Exit => Pin::dir_set(),
        };
    }

    fn set_puls_pin() {
        PulsPin::puls_set();
    }

    fn set_pwm_freq(freq: u32) {
        PWM::tim2_pwm_set_freq(freq);
    }

    fn wait_run_finish() {
        _ = EVENT_FLAGS.wait_any_flags(FLAG_FINISH, osWaitForever);
    }

    fn notify_run_finish() {
        _ = EVENT_FLAGS.set_flags(FLAG_FINISH);
    }
}

pub const MAX_PULS_COUNT: u32 = 483 * 6;

pub unsafe fn init() {
    EVENT_FLAGS.init_once("pick_motor");
    PickMotorDrv::notify_run_finish();

    Pin::gpio_init();
    PulsPin::gpio_init();
    PWM::pwm_init();

    Pin::enable_reset();
    PWM::tim2_pwm_disable_irq();
    PWM::tim2_pwm_disable();

    NVIC::unmask(Interrupt::TIM2);
}

pub fn is_alm() -> bool {
    PickMotorDrv::is_alm()
}

// pub fn async_run(position: u32) -> Result<(), CargoErr> {
//     unsafe { PICK_MOTOR.as_mut() }.run_position_with_async(position)
// }

pub fn run(position: u32) -> Result<(), CargoErr> {
    unsafe { PICK_MOTOR.as_mut() }.run_position(position as i32)
}

#[interrupt]
unsafe fn TIM2() {
    if PWM::tim2_is_irq() {
        PWM::tim2_clear_irq();
        PICK_MOTOR.as_mut().on_puls();
    }
}



